#ifndef CURVEFITTER_H
#define CURVEFITTER_H

#include <iostream>
#include <opencv2/opencv.hpp>
#include <opencv2/core/ocl.hpp>
#include <numeric>
#include "manifoldCommunication.h"
#include "minAreaRect.h"
#include "customTimer.h"
#include <chrono>
#include <vector>
#include <deque>
using namespace cv;
using namespace std;

class sinLeastSquare: public cv::MinProblemSolver::Function
{
public:
    sinLeastSquare(deque<Point2f> dataV, double omega);
    int getDims() const;
    double calc(const double* arg)const;
    void getGradient(const double* arg,double* grad);

private:
    deque<Point2f> dataV; 
    double omega; 
};

class CurveFitter
{
public:
    CurveFitter(const int fitLineNumThreshold=40, 
                const int fitLineNumUpperBound=200,
                const int fitSinNumThreshold=10,
                const int fitSinNumUpperBound=40);

    void curveFitting(Point2f armorBoxCenter,
                    Point2f& predictArmorBoxCenter,
                    const double circleRadius=0.,
                      const Point2f inputBias=Point2f(0,0), 
                      const double delta_t_ms=500.
                      );

    void initSet(const Point2f powerRuneCircleCenter, const double baseAngle);

    bool isFirstSetBaseAngle() const; 

    void setPowerRuneCircleCenter(const Point2f newCircleCenter); 
    Point2f getPowerRuneCircleCenter() const; 

    void inputCurveFitDataV(deque<Point2f>& circleDataQue); // circleData: <t_s, armorBoxAngle>

private:
    int fitLineNumThreshold, fitLineNumUpperBound, fitSinNumThreshold, fitSinNumUpperBound;
    deque<Point2f> curveFitDataV;
    deque<Point2f> fitSinDataV; 
    void curveFittingDebug(Mat& canvas);
    double base_angle;
    double startTime; 
    Point2f powerRuneCircleCenter;  
    cv::Vec4f fitLinePara;
    double fitLineK, sinA, sinB, sinOmega, sinMean; 
    cv::Ptr<cv::ConjGradSolver> solver; // for fit sine curve
    cv::Ptr<cv::MinProblemSolver::Function> ptr_F;

    void fitLine();
    void fitSin(deque<Point2f> dataQue);
    Point2f predictNewArmorBoxCenter(const double delta_t_ms, const Point2f currentArmorBoxCenter,const double currentAngle=0.,const double circleRadius=0., const Point2f inputBias=Point2f(0,0));
    double getFitAngle(double t_s); 
};

#endif //CURVEFITTER_H